/*
 * ekf.hpp
 *
 *  Created on: 2021年4月17日
 *      Author: Zhiping Fu
 */

#ifndef MODULES_EKF_ECL_EKF_HPP_
#define MODULES_EKF_ECL_EKF_HPP_


#include "system/system.hpp"
#include "userlib/userlib.hpp"


#ifdef __cplusplus

#include <Eigen>

using namespace std;
using namespace Eigen;

class EKF
{
public:
	EKF(){};
	EKF(char *n) : name(n){}
	~EKF(){};

	void ekfInit();
	void ekfRun();

private:
	char *name;
	float              dt;
	uint32_t           startTimer;
	uint32_t           stopTimer;
	uint32_t           executionTime_us;
	uint32_t           startTimerLast;
	uint32_t           cycleTime_us;

	float              gravity;

	Vector3f MatAcc; //加速度计数据
	Vector3f MatGyr; //陀螺仪数据
	Vector3f MatMag; //磁力计数据

	Matrix<float,10,1> X_p;		//预测状态
	Matrix<float,10,1> X_k;		//更新状态
	Quaternion<float>  q_p;  		//预测四元数
	Quaternion<float>  q_k;  		//更新四元数
	Vector3f           X_vel_p;		//NED速度预测
	Vector3f           X_vel_k;		//NED速度更新
	Vector3f           X_pos_p;		//NED位置预测
	Vector3f           X_pos_k;		//NED位置更新
	Matrix<float, 10, 10> P_p;		//预测协方差矩阵
	Matrix<float, 10, 10> P_k;		//更新协方差矩阵

	Matrix<float, 10, 10> F;
	Matrix<float, 10, 6>  G;
	Matrix<float, 10, 10> N_process;
	Matrix<float, 6, 6>   Q;

	Vector3f              z_uwb;
	Matrix<float, 3, 10>  H_uwb;
	Matrix<float, 10, 3>  K_uwb;
	Vector3f              z_uwb_p;
	Matrix<float, 3, 3>   R_uwb;

	Matrix<float, 3, 3>   Cb2n;
	Vector3f              Euler;  //欧拉角

	float                 z_yaw;
	Matrix<float, 1, 10>  H_yaw;
	float                 z_yaw_p;
	float                 z_yaw_sub_z_yaw_p;
	Matrix<float, 10, 1>  K_yaw;
	float                 R_yaw;
	Matrix<float, 3, 3>   R_tilt;


	uwb_pos_msg uwbPos;
	sensor_mag_msg mag;
	sensor_acc_msg acc;
	height_msg height;
	delta_angle_and_vel_msg delta_angle_and_vel;
	delta_to_zeros_flag_msg delta_to_zeros_flag;
	ekf_msg ekf;



};


#endif



#endif /* MODULES_EKF_ECL_EKF_HPP_ */
